# -*- coding: utf-8 -*-
from typing import Dict, Any, Tuple
import numpy as np
from recognition.actions.base import BaseAction
from common.landmarks import LMS
from common.config import (
    SQUAT_KNEE_PROX_DELTA, SQUAT_KNEE_ANGLE_MAX,
    SQUAT_HIP_ANKLE_GAP_MAX, SQUAT_HIP_DROP_VS_SHOULDER,
    VIS_THRESH
)

class SquatAction(BaseAction):
    label = "squat"

    def score(self, lms, person_id: int, get_state) -> Tuple[float, Dict[str, Any]]:
        """
        融合多特征的瞬时评分 + 轻微 EMA 平滑：
        - 髋-膝 y 接近（hip_y > knee_y - delta）
        - 膝角小于阈值（至少一侧可见）
        - 髋-踝间距缩小（可见踝均值）
        - 髋相对肩明显下降（辅助）
        最终将 3 个主判据 + 1 个辅助映射成分数，并用少量 EMA 平滑。
        """
        try:
            l_sh = lms[LMS.LEFT_SHOULDER]; r_sh = lms[LMS.RIGHT_SHOULDER]
            l_hp = lms[LMS.LEFT_HIP];      r_hp = lms[LMS.RIGHT_HIP]
            l_kn = lms[LMS.LEFT_KNEE];     r_kn = lms[LMS.RIGHT_KNEE]
            l_an = lms[LMS.LEFT_ANKLE];    r_an = lms[LMS.RIGHT_ANKLE]

            def ok(p): return getattr(p, "visibility", 0.0) >= VIS_THRESH

            left_knee_ok  = ok(l_kn)
            right_knee_ok = ok(r_kn)
            left_hip_ok   = ok(l_hp)
            right_hip_ok  = ok(r_hp)
            left_ankle_ok = ok(l_an)
            right_ankle_ok= ok(r_an)

            knees_ok = left_knee_ok and right_knee_ok
            hips_ok  = left_hip_ok and right_hip_ok
            any_ankle_ok = left_ankle_ok or right_ankle_ok
            if not (knees_ok and hips_ok):
                return 0.0, {}

            # 归一化 y（向下为正）
            hip_y  = 0.5 * (l_hp.y + r_hp.y)
            knee_y = 0.5 * (l_kn.y + r_kn.y)
            sh_y   = 0.5 * (l_sh.y + r_sh.y)

            # 1) 髋-膝接近
            knee_proximity_ok = (hip_y > knee_y - SQUAT_KNEE_PROX_DELTA)

            # 2) 膝角（使用归一化坐标近似）
            knee_angles = []
            if left_ankle_ok:
                knee_angles.append(self._angle_approx(l_hp, l_kn, l_an))
            if right_ankle_ok:
                knee_angles.append(self._angle_approx(r_hp, r_kn, r_an))
            knee_angle_ok = (len(knee_angles) >= 1 and all(ka < SQUAT_KNEE_ANGLE_MAX for ka in knee_angles))

            # 3) 髋-踝间距
            hip_ankle_ok = False
            if any_ankle_ok:
                ank_y_list = []
                if left_ankle_ok:  ank_y_list.append(l_an.y)
                if right_ankle_ok: ank_y_list.append(r_an.y)
                ankle_y = float(np.mean(ank_y_list))
                gap = hip_y - ankle_y
                hip_ankle_ok = (gap < SQUAT_HIP_ANKLE_GAP_MAX)

            # 辅助：髋相对肩明显下降
            hip_drop_vs_shoulder_ok = (hip_y > sh_y + SQUAT_HIP_DROP_VS_SHOULDER)

            # 投票合成分数（主判据 3 票 + 辅助 0.5 票）
            votes = 0
            votes += 1 if knee_proximity_ok else 0
            votes += 1 if knee_angle_ok else 0
            if any_ankle_ok:
                votes += 1 if hip_ankle_ok else 0
            aux = 0.5 if hip_drop_vs_shoulder_ok else 0.0

            raw = (votes + aux) / 3.5  # 最大 3.5 分 → 归一化
            raw = float(np.clip(raw, 0.0, 1.0))

            # 轻微历史平滑（每人单独的 EMA）
            st = get_state(person_id)
            prev = st.get("_squat_ema", None)
            alpha = 0.4  # 平滑系数（越大越贴近当前帧）
            smoothed = raw if prev is None else (prev * (1 - alpha) + raw * alpha)
            st["_squat_ema"] = smoothed

            return float(smoothed), {}
        except Exception:
            return 0.0, {}

    @staticmethod
    def _angle_approx(a, b, c) -> float:
        """
        使用归一化坐标近似 ∠ABC（B 为顶点）。像素坐标更精确，这里近似足够用于打分。
        """
        import numpy as np
        v1 = np.array([a.x - b.x, a.y - b.y], dtype=np.float32)
        v2 = np.array([c.x - b.x, c.y - b.y], dtype=np.float32)
        n1 = np.linalg.norm(v1) + 1e-6
        n2 = np.linalg.norm(v2) + 1e-6
        cosv = np.clip(np.dot(v1, v2) / (n1 * n2), -1.0, 1.0)
        return float(np.degrees(np.arccos(cosv)))